#include <Arduino.h>
#include <servo.h>
#include <eeprom.h>
#include "flexible_button.h"

#define MAX_STATIC_POINT 430
#define MIN_STATIC_POINT 330
#define MID_STATIC_POINT (MAX_STATIC_POINT + MIN_STATIC_POINT) / 2

#define MIN_STATIC_POINT2 100

#define ORG_OFFSET_VOLTAGE  2.5
#define REG_OUTPUT          7.5
#define REG_LOW_PULL        13.0
// #define OFFSET_VOLTAGE      (ORG_OFFSET_VOLTAGE * (REG_LOW_PULL / (REG_OUTPUT + REG_LOW_PULL)))
// #define OFFSET_AD_VALUE     (OFFSET_VOLTAGE * 1024 / 3.3)
// #define MAX_AD_VALUE        (OFFSET_AD_VALUE * 2)
#define OFFSET_VOLTAGE      1.575
#define OFFSET_AD_VALUE     488
#define MAX_AD_VALUE        976
#define NOISE_RANGE         20

#define SAMPLE_COUNT    30
#define DEFAULT_SPEED 30 //采样点数
#define DEFAULT_RUN_SPEED (1500 / 180)
// #define SetAngleTimeMs(newAngle, lastAngle) (ceil(abs(newAngle - lastAngle) / DEFAULT_RUN_SPEED))
// #define SetAngleTimeMs(newAngle, lastAngle) (ceil((abs(newAngle - lastAngle) - 180) * (abs(newAngle - lastAngle) - 180) * (-0.046) + 1500))   //y = -0.046 * x^2 + 1500
#define SetAngleTimeMs(newAngle, lastAngle) (ceil((abs(newAngle - lastAngle) - 180) * (abs(newAngle - lastAngle) - 180) * (-0.0308) + 1000))   //y = -0.046 * x^2 + 1500

#define MIC_PIN     PIN_A0
#define PRES_PIN    PIN_A1
#define SERVO_PIN   PIN_A6
#define LED1        PB5
#define LED2        PB4
#define LED3        PB3

#define KEY_MODE PB12
#define KEY_SET  PB13
#define KEY_PRID PB14
#define KEY_NEXT PB15

#define EEPROM_FLAG_TRUE        0xA5
#define EEPROM_RANGE_EN_ADDR    0x00
#define EEPROM_MIN_RANGE_ADDR   0x01
#define EEPROM_MAX_RANGE_ADDR   0x02
#define EEPROM_SPEED_EN_ADDR    0x03
#define EEPROM_SPEED_ADDR       0x04

#define EEPROM_RECORD_ADDR1     0x05
#define EEPROM_RECORD_ADDR2     (EEPROM_RECORD_ADDR1 + 64 + 1)
#define EEPROM_RECORD_ADDR3     (EEPROM_RECORD_ADDR2 + 64 + 1)
#define EEPROM_RECORD_ADDR4     (EEPROM_RECORD_ADDR3 + 64 + 1)
#define EEPROM_RECORD_ADDR5     (EEPROM_RECORD_ADDR4 + 64 + 1)
#define EEPROM_RECORD_ADDR6     (EEPROM_RECORD_ADDR5 + 64 + 1)
#define EEPROM_RECORD_ADDR7     (EEPROM_RECORD_ADDR6 + 64 + 1)
uint8_t gMinRange;
uint8_t gMaxRange;
uint8_t gSpeed;

int16_t gCurentAngle = 0;
int16_t gLastAngle   = 0;
uint16_t gFinishTimeInterval = 0;
uint32_t gLastTime   = 0;

typedef struct{
    uint8_t Nums;
    uint8_t Table[64];
} AngleRecord_t;
AngleRecord_t gAngleRecord[7] = {0};
AngleRecord_t* gSelectAngle = NULL;
static uint16_t gAngleAddrTable[] = {
    EEPROM_RECORD_ADDR1, EEPROM_RECORD_ADDR2, EEPROM_RECORD_ADDR3, EEPROM_RECORD_ADDR4,
    EEPROM_RECORD_ADDR5, EEPROM_RECORD_ADDR6, EEPROM_RECORD_ADDR7
};

typedef enum : uint8_t {
    KEY_MODE_IDX = 0,
    KEY_SET_IDX ,
    KEY_PRID_IDX,
    KEY_NEXT_IDX,
    USER_BUTTON_MAX
} user_button_t;
static flex_button_t user_button[USER_BUTTON_MAX];

typedef enum : uint8_t {
    WorkModeVoice = 0,
    WorkModePReg,
    WorkModeTableOnce,
    WorkModeTableLoop,
    WorkModeTableOneByOne,
    WorkModeSetting,
    WorkModeMax
} WorkMode_t;
WorkMode_t WorkMode;
uint8_t SettingStatus = 0;

Servo MainServo;  // create servo object to control a servo
bool servo_runing = false;
uint8_t zero_dect_cnt = 0;
uint8_t valid_dect_cnt = 0;
bool zero_dect_flag = false;
bool valid_dect_flag = false;
uint8_t sys_status = 0;
bool angle_used = false;

bool    Time20msIF;
uint8_t Tim20msCnt;
bool    Time100msIF;
uint8_t Tim100msCnt;
bool    Time200msIF;
uint8_t Tim200msCnt;
bool    Time500msIF;
uint8_t Tim500msCnt;
bool    Time1000msIF;
uint8_t Tim1000msCnt;

void SoftTimeDeal();
void KeyModeHandler(void* arg);
void KeySetHandler(void* arg);
void KeyPridHandler(void* arg);
void KeyNextHandler(void* arg);
uint16_t SetAngle(int16_t angle);

static uint8_t KeyModeRead(void* arg){
    return (uint8_t)digitalRead(KEY_MODE);
}

static uint8_t KeySetRead(void* arg){
    return (uint8_t)digitalRead(KEY_SET);
}

static uint8_t KeyPridRead(void* arg){
    return (uint8_t)digitalRead(KEY_PRID);
}

static uint8_t KeyNextRead(void* arg){
    return (uint8_t)digitalRead(KEY_NEXT);
}

bool user_button_init(){
    memset(&user_button[0], 0x00, sizeof(user_button));
    pinMode(KEY_MODE, INPUT_PULLUP);
    pinMode(KEY_SET , INPUT_PULLUP);
    pinMode(KEY_PRID, INPUT_PULLUP);
    pinMode(KEY_NEXT, INPUT_PULLUP);

    user_button[0].usr_button_read = KeyModeRead;
    user_button[0].cb = KeyModeHandler;
    user_button[1].usr_button_read = KeySetRead;
    user_button[1].cb = KeySetHandler;
    user_button[2].usr_button_read = KeyPridRead;
    user_button[2].cb = KeyPridHandler;
    user_button[3].usr_button_read = KeyNextRead;
    user_button[3].cb = KeyNextHandler;
    for (uint8_t i = 0; i < USER_BUTTON_MAX; i++) {
        user_button[i].id = i;
        user_button[i].pressed_logic_level = 0;
        user_button[i].short_press_start_tick = FLEX_MS_TO_SCAN_CNT(1500);
        user_button[i].long_press_start_tick = FLEX_MS_TO_SCAN_CNT(3000);
        user_button[i].long_hold_start_tick = FLEX_MS_TO_SCAN_CNT(4500);
        flex_button_register(&user_button[i]);
    }
    bool key1 = true;
    bool key2 = true;
    while (key1 && key2) {
        if(Time20msIF){
            key1 = !digitalRead(KEY_MODE);
            key2 = !digitalRead(KEY_SET);
        }
        if(Time1000msIF){
            return true;    //长按时间超过1s，则认为触发自检
        }
        SoftTimeDeal();
    }
    return false;
}

uint16_t GetPRegAngle(){
    uint16_t angle = analogRead(PRES_PIN);
    angle = map(angle, 0, 959, 0, 180);
    return angle;
}

void SaveAngleTable(){
    for(uint8_t i = 0; i < 7; i++){
        gSelectAngle = &gAngleRecord[i];
        EEPROM.write(gAngleAddrTable[i], gSelectAngle->Nums);
        for(uint8_t j = 0; j < gSelectAngle->Nums; j++){
            EEPROM.write(gAngleAddrTable[i] + j + 1, gMaxRange);
        }
    }
}

bool gTableRunStatus = false;
uint8_t gTableIdx = 0;
uint8_t gStepIdx  = 0;
void KeyModeHandler(void* arg){
    if (flex_button_event_read(&user_button[KEY_MODE_IDX]) == FLEX_BTN_PRESS_CLICK) {
        WorkMode_t WorkModeBak = (WorkMode_t)(WorkMode + 1);
        if(WorkMode == WorkModeSetting) return;
        if(WorkModeBak >= WorkModeSetting)
            WorkMode = WorkModeVoice;
        else
            WorkMode = WorkModeBak;
        if(WorkMode == WorkModeVoice){
            digitalWrite(LED2, 0);
            digitalWrite(LED3, 0);
        } else if (WorkMode == WorkModePReg) {
            digitalWrite(LED1, 0);
            digitalWrite(LED3, 0);
        } else if (WorkMode == WorkModeTableOnce) {
            digitalWrite(LED1, 0);
            digitalWrite(LED2, 0);
            gTableRunStatus = false;
            gTableIdx = 0;
            gStepIdx  = 0;
            gSelectAngle = &gAngleRecord[gTableIdx];
            Serial1.print("Table[");
            Serial1.print(gTableIdx);
            Serial1.print("].Nums = ");
            Serial1.println(gSelectAngle->Nums);
            SetAngle(0);
        } else if (WorkMode == WorkModeTableLoop) {
            digitalWrite(LED3, 1);
        }
    } else if (flex_button_event_read(&user_button[KEY_MODE_IDX]) == FLEX_BTN_PRESS_SHORT_START) {
        if(WorkMode == WorkModeSetting){
            SaveAngleTable();
            WorkMode = WorkModeVoice;
        } else{
            WorkMode = WorkModeSetting;
            SettingStatus = 0;
            gTableIdx = 0;
            gStepIdx  = 0;
            gSelectAngle = &gAngleRecord[gTableIdx];
            Serial1.print("Table[");
            Serial1.print(gTableIdx);
            Serial1.print("].Nums = ");
            Serial1.println(gSelectAngle->Nums);
        }
    }
}

void KeySetHandler(void* arg){
    if (flex_button_event_read(&user_button[KEY_SET_IDX]) == FLEX_BTN_PRESS_CLICK) {
        if(WorkMode == WorkModeTableOnce){
            //重新跑一次
            gTableRunStatus = false;
            gTableIdx = 0;
            gStepIdx  = 0;
        } else if(WorkMode == WorkModeSetting){
            //录入角度
            gSelectAngle->Table[gStepIdx] = GetPRegAngle();
            if(++gStepIdx >= 64){
                SettingStatus = 1;
                gStepIdx = 0;
            }
            gSelectAngle->Nums = gStepIdx;
        }
    } else if (flex_button_event_read(&user_button[KEY_SET_IDX]) == FLEX_BTN_PRESS_SHORT_START) {
        if(WorkMode == WorkModeSetting){
            //保存角度列表
            SaveAngleTable();
        }
    }
}

void KeyPridHandler(void* arg){
    if((WorkMode < WorkModeTableOnce)){// || (WorkMode > WorkModeTableLoop)
        return;
    }
    if (flex_button_event_read(&user_button[KEY_PRID_IDX]) == FLEX_BTN_PRESS_CLICK) {
        gStepIdx = 0;
        SettingStatus = 0;
        gTableIdx--;
        if(gTableIdx == 255){
            gTableIdx = 6;
        }
        gSelectAngle = &gAngleRecord[gTableIdx];
        Serial1.print("Table[");
        Serial1.print(gTableIdx);
        Serial1.print("].Nums = ");
        Serial1.println(gSelectAngle->Nums);
    }
}

void KeyNextHandler(void* arg){
    if((WorkMode < WorkModeTableOnce)){// || (WorkMode > WorkModeTableLoop)
        return;
    }
    if (flex_button_event_read(&user_button[KEY_NEXT_IDX]) == FLEX_BTN_PRESS_CLICK) {
        gStepIdx = 0;
        SettingStatus = 0;
        gTableIdx++;
        if(gTableIdx > 6){
            gTableIdx = 0;
        }
        gSelectAngle = &gAngleRecord[gTableIdx];
        Serial1.print("Table[");
        Serial1.print(gTableIdx);
        Serial1.print("].Nums = ");
        Serial1.println(gSelectAngle->Nums);
    }
}

void SoftTimeDeal(){
    static uint32_t aSoftwareTimeCntBak = 0;
    uint32_t now = millis();
    uint32_t timeInterval = now - aSoftwareTimeCntBak;

    Time20msIF   = false;
    Time100msIF  = false;
    Time200msIF  = false;
    Time500msIF  = false;
    Time1000msIF = false;
    if (timeInterval >= 20) { // 20ms
        Time20msIF = true;
        if(timeInterval >= 40){
            aSoftwareTimeCntBak = now;
            uint8_t timeSyncCnt = (timeInterval / 20) - 1;
            Tim20msCnt   += timeSyncCnt;
            Tim100msCnt  += timeSyncCnt;
            Tim200msCnt  += timeSyncCnt;
            Tim500msCnt  += timeSyncCnt;
            Tim1000msCnt += timeSyncCnt;
        }
        else{
            aSoftwareTimeCntBak += 20;
        }
        if (++Tim100msCnt >= 5) {
            Tim100msCnt = 0;
            Time100msIF = true;
        }
        if (++Tim200msCnt >= 10) {
            Tim200msCnt = 0;
            Time200msIF = true;
        }
        if (++Tim500msCnt >= 25) {
            Tim500msCnt = 0;
            Time500msIF = true;
        }
        if (++Tim1000msCnt >= 50) {
            Tim1000msCnt = 0;
            Time1000msIF = true;
        }
    }
}

void SettingDeal(){
    if(Serial1.available()){
        String data = Serial1.readString();
        data.toLowerCase();
        Serial1.print("cmd: ");
        Serial1.println(data);
        if(data == "exit"){
            WorkMode == WorkModeVoice;
            Serial1.println("exit ok");
        }
        else if(data.indexOf("go") >= 0){
            uint8_t angle_temp = data.substring(data.indexOf("[") + 1, data.indexOf("]")).toInt();
            if(angle_temp > 180) angle_temp = 180;
            SetAngle(angle_temp);
            WorkMode = WorkModeSetting;
            Serial1.print("go ");
            Serial1.println(angle_temp);
        }
        else if(data.indexOf("range") >= 0){
            gMinRange = data.substring(data.indexOf("[") + 1, data.indexOf(",")).toInt();
            gMaxRange = data.substring(data.indexOf(",") + 1, data.indexOf("]")).toInt();
            if(gMinRange > gMaxRange){
                uint8_t temp = gMinRange;
                gMinRange = gMaxRange;
                gMaxRange = temp;
            }
            EEPROM.write(EEPROM_RANGE_EN_ADDR, EEPROM_FLAG_TRUE);
            EEPROM.write(EEPROM_MIN_RANGE_ADDR, gMinRange);
            EEPROM.write(EEPROM_MAX_RANGE_ADDR, gMaxRange);
            WorkMode = WorkModeSetting;
            Serial1.print("set ");
            Serial1.print(gMinRange);
            Serial1.print(",");
            Serial1.println(gMaxRange);
        }
        else if(data.indexOf("speed") >= 0){
            gSpeed = data.substring(data.indexOf("[") + 1, data.indexOf("]")).toInt();
            if(gSpeed < 10 || gSpeed > 100){
                gSpeed = DEFAULT_SPEED;
            }
            EEPROM.write(EEPROM_SPEED_EN_ADDR, EEPROM_FLAG_TRUE);
            EEPROM.write(EEPROM_SPEED_ADDR, gSpeed);
            WorkMode = WorkModeSetting;
            Serial1.print("set speed ");
            Serial1.println(gSpeed);
        }
        else if(data == "get"){
            WorkMode = WorkModeSetting;
            Serial1.print("get range:[");
            Serial1.print(gMinRange);
            Serial1.print(",");
            Serial1.print(gMaxRange);
            Serial1.println("]");
        }
    }
}

bool CheckServoFinish(){
    return ((millis() - gLastTime) > gFinishTimeInterval);
}

int16_t GetAngle(){
    if(CheckServoFinish())
        return gCurentAngle;
    else{
        uint32_t timeInterval = millis() - gLastTime;
        int16_t  angle = timeInterval * DEFAULT_RUN_SPEED;
        if(gCurentAngle > gLastAngle){
            angle = gLastAngle + angle;
            if(angle > gCurentAngle){
                angle = gCurentAngle;
            }
        }
        else{
            angle = gLastAngle - angle;
            if(angle < gCurentAngle){
                angle = gCurentAngle;
            }
        }
        return angle;
    }
}

uint16_t SetAngle(int16_t angle){
    // Serial1.print("LastAngle: ");
    // Serial1.print(gLastAngle);
    // Serial1.print(", SetAngle: ");
    // Serial1.print(angle);
    MainServo.write(angle);
    gLastTime = millis();
    gFinishTimeInterval = SetAngleTimeMs(angle, gLastAngle);
    // Serial1.print(", delay: ");
    // Serial1.println(gFinishTimeInterval);
    gLastAngle = angle;
    // gCurentAngle = angle;
    return gFinishTimeInterval;
}

int16_t SampleAudio(bool get){
    static uint8_t idx = 0;
    static uint16_t gSampleTable[SAMPLE_COUNT] = {0};
    int16_t result = 0;
    if(get){
        uint8_t sampleCount = 0;
        for(uint8_t i = 0; i < SAMPLE_COUNT; i++){
            if(gSampleTable[i] > NOISE_RANGE){
                result = gSampleTable[i] + result;
                sampleCount++;
            }
        }
        if(sampleCount > (SAMPLE_COUNT >> 1)){
            for(uint8_t i = 0; i < SAMPLE_COUNT; i++){
                gSampleTable[i] = 0;
            }
            result = result / sampleCount;
            idx = 0;
        }
    }
    int16_t temp = analogRead(MIC_PIN);
    
    Serial1.print("$ ");
    Serial1.print(temp);
    if(temp > 5){
        gSampleTable[idx] = abs(temp - OFFSET_AD_VALUE);
    } else{                                                         // APM OFF, always output 0
        gSampleTable[idx] = 0;
    }
    Serial1.print("-> ");
    Serial1.println(gSampleTable[idx]);
    if(++idx >= SAMPLE_COUNT){
        idx = 0;
    }
    return result;
}

bool RunOnceTable(){
    static int8_t sampleCnt = 0;
    if(gSelectAngle->Nums == 0) return true;

    if(!gTableRunStatus){
        if(CheckServoFinish()){
            uint8_t angle = gSelectAngle->Table[gStepIdx];
            SetAngle(angle);
            if(++gStepIdx >= gSelectAngle->Nums){
                gStepIdx = 0;
                gTableRunStatus = true;
            }
        }
    } 
    return gTableRunStatus;
}

void WorkModePRegDeal(){
    if(Time100msIF){
        static uint16_t lastAngle = 0;
        uint16_t angle = GetPRegAngle();
        if(angle != lastAngle){
            SetAngle(angle);
            lastAngle = angle;
            Serial1.print("Set angle: ");
            Serial1.println(angle);
        }
    }
}

void WorkModeDeal(){
    SettingDeal();
    if (WorkMode == WorkModeVoice) {
        static bool isTrunBack = false;
        if(Time20msIF){
            SampleAudio(false);
        } else if(CheckServoFinish()){
            if(isTrunBack){
                isTrunBack = false;
                SetAngle(gMinRange);
            } else{
                int16_t angle = SampleAudio(true);
                if(angle > NOISE_RANGE){
                    // angle = int16_t(35 * log10(angle + 4.5) - 34);  //当前5-488->0-60，公式需要改5-488->0-180
                    angle = 0.00000165 * (angle-490) * (angle-490) * (angle-490) + 185;  //(0.00000165)(𝑥−490)(𝑥−490)(𝑥−490)+185
                    if(angle > 180) angle = 180;
                    else if(angle < 0) angle = 0;
                    angle = map(angle, 0, 180, gMinRange, gMaxRange);
                }
                SetAngle(angle);
                isTrunBack = true;
            }
            // SetAngle(0);
        }
    } else if (WorkMode == WorkModePReg) {
        WorkModePRegDeal();
    } else if (WorkMode == WorkModeTableOnce) {
        RunOnceTable();
    } else if (WorkMode == WorkModeTableLoop) {
        bool check_run = RunOnceTable();
        if(check_run){
            gTableRunStatus = false;
            gStepIdx = 0;
        }
    } else if (WorkMode == WorkModeTableOneByOne) {
        bool check_run = RunOnceTable();
        if(check_run){
            gTableRunStatus = false;
            gStepIdx = 0;
            if(++gTableIdx > 6){
                gTableIdx = 0;
            }
            gSelectAngle = &gAngleRecord[gTableIdx];
        }
    } else if (WorkMode == WorkModeSetting) {
        WorkModePRegDeal();
    }
}

void DisplayDeal(){
    static bool led_status  = false;
    static bool led_status2 = false;
    static uint8_t led_pos  = 0x01;
    if(Time500msIF){
        led_status = !led_status;
        if(WorkMode == WorkModeVoice){
            digitalWrite(LED1, led_status);
        } else if (WorkMode == WorkModePReg) {
            digitalWrite(LED2, led_status);
        } else if (WorkMode == WorkModeTableOnce) {
            digitalWrite(LED3, led_status);
        } else if (WorkMode == WorkModeTableLoop) {
            // digitalWrite(LED3, led_status);
        } else if (WorkMode == WorkModeTableOneByOne) {
            digitalWrite(LED1, led_pos & 0x01);
            digitalWrite(LED2, led_pos & 0x02);
            digitalWrite(LED3, led_pos & 0x04);
            led_pos = led_pos << 1;
            if(led_pos > 0x04){
                led_pos = 0x01;
            }
        }
    }
    if(Time200msIF){
        if (WorkMode == WorkModeSetting) {
            led_status2 = !led_status2;
            led_pos = gTableIdx + 1;
            if(SettingStatus == 0){
                digitalWrite(LED1, (led_pos & 0x01) && led_status2);
                digitalWrite(LED2, (led_pos & 0x02) && led_status2);
                digitalWrite(LED3, (led_pos & 0x04) && led_status2);
            } else if(SettingStatus == 1){
                digitalWrite(LED1, led_pos & 0x01);
                digitalWrite(LED2, led_pos & 0x02);
                digitalWrite(LED3, led_pos & 0x04);
            }
        }
    }
}

void hw_init(){
    Serial1.begin(115200);
    // Serial1.attachInterrupt(SerialDeal);
    pinMode(LED1, OUTPUT);
    digitalWrite(LED1, 0);
    pinMode(LED2, OUTPUT);
    digitalWrite(LED2, 0);
    pinMode(LED3, OUTPUT);
    digitalWrite(LED3, 0);
    pinMode(MIC_PIN, INPUT_ANALOG);
    pinMode(PRES_PIN, INPUT_ANALOG);
    MainServo.attach(SERVO_PIN, 500, 2500);  // attaches the servo on pin 9 to the servo object
    MainServo.write(0);
    gCurentAngle = 0;
    gLastAngle = 0;
    delay(1500);
    user_button_init();
}

void sys_init(){
    WorkMode = WorkModeVoice;
    if(EEPROM.read(EEPROM_RANGE_EN_ADDR) == EEPROM_FLAG_TRUE){
        Serial1.println("EEPROM Range Enable");
        gMinRange = EEPROM.read(EEPROM_MIN_RANGE_ADDR);
        gMaxRange = EEPROM.read(EEPROM_MAX_RANGE_ADDR);
    } else {
        gMinRange = 0;
        gMaxRange = 60;
    }
    if(EEPROM.read(EEPROM_SPEED_EN_ADDR) == EEPROM_FLAG_TRUE){
        gSpeed = EEPROM.read(EEPROM_SPEED_ADDR);
    }
    else{
        gSpeed = DEFAULT_SPEED;
    }
    if(gSpeed < 10 || gSpeed > 50){
        gSpeed = DEFAULT_SPEED;
    }
    for(uint8_t i = 0; i < 7; i++){
        uint8_t nums = EEPROM.read(gAngleAddrTable[i]);
        if(nums > 64){
            gAngleRecord[i].Nums = 0;
        }
        else{
            gAngleRecord[i].Nums = nums;
            for(uint8_t j = 0; j < nums; j++){
                gAngleRecord[i].Table[j] = EEPROM.read(gAngleAddrTable[i] + j + 1);
                if(gAngleRecord[i].Table[j] > 180) gAngleRecord[i].Table[j] = 180;
            }
        }
    }
    SetAngle(gMinRange);
    Serial1.print("Speed: ");
    Serial1.println(gSpeed);
    Serial1.print("MinRange: ");
    Serial1.print(gMinRange);
    Serial1.print(", MaxRange: ");
    Serial1.println(gMaxRange);
}

void setup() {
    hw_init();
    sys_init();
}
void loop() {
  // put your main code here, to run repeatedly:
    WorkModeDeal();
    DisplayDeal();
    if (Time20msIF) {
        flex_button_scan();
    }
    SoftTimeDeal();
}